import serial
import crcmod
import time
from rclpy.node import Node
class laserScan(Node):
    def __init__(self,port,baudrate):
        super().__init__("laser_scan_dis_node")
        try:
            self.ser = serial.Serial(
                port=port,              # 替换为实际串口号
                baudrate=baudrate,          # 默认波特率
                timeout=1                 # 超时时间（秒）
            )
            self.get_logger().info(f"串口已打开: {port}")
        except serial.SerialException as e:
            self.get_logger().warning(f"无法打开串口: {e}")
            self.get_logger().warning('-----------------sls------------------')
            self.get_logger().warning('检查端口号，检查波特率')            
            self.get_logger().warning("-----------------sls------------------")
            return

        # CRC16配置（多项式0x8005，初始值0xFFFF，输入输出反转）
        self.crc16 = crcmod.mkCrcFun(0x18005, initCrc=0xFFFF, rev=True, xorOut=0x0000)

    def build_frame(self, cmd, data=bytes()):
        header = bytes([0xA5, 0x03, 0x20, cmd, 0x00])
        length = len(data).to_bytes(2, 'big')
        frame = header + length + data
        crc = self.crc16(frame).to_bytes(2, 'big')
        return frame + crc

    def start_measure(self):
        try:
            start_measure_frame = self.build_frame(0x01)
            self.ser.write(start_measure_frame)
        except:
            self.get_logger().warning("find sys healthy")
        # print(start_measure_frame.hex())

    def get_fps(self):
        get_fps_frame = self.build_frame(0x01B)
        self.ser.write(get_fps_frame)
        fps_resp = b''
        while fps_resp == b'':
            fps_resp = laser.ser.read_all()
        if fps_resp[-3] ==0x00:
            return 50
        elif fps_resp[-3] ==0x01:
            return 100
        elif fps_resp[-3] ==0x02:
            return 250
        else:
            return -1

    def get_distance(self):
        try:
            # start_indices = None
            if self.ser.in_waiting > 0:  # 检查缓冲区是否有数据
                data = self.ser.read(self.ser.in_waiting)  # 读取所有可用数据
                # print(bytes(data).hex())
                prefix = b'\xa5\x03'
                start_indices = [i for i in range(len(data)) if data.startswith(prefix, i)]
                # 找到最新的索引
                if start_indices:
                    latest_index = start_indices[-1]
                    # 找到最新的数据包
                    latest_data = data[latest_index:data.find(b'\xa5\x03', latest_index + 1) if data.find(b'\xa5\x03', latest_index + 1) != -1 else None]
                    # print(latest_data[-9])
                    if latest_data[-9] == 255:
                        return -1
                    high = latest_data[-9]<<8|latest_data[-10]
                    return -1 if high > 32767 else high
                else:
                    return -2
            else:
                return -2
        except:
            return -3
        
    def stop_measure(self):
        stop_cmd = self.build_frame(0x02)
        self.ser.write(stop_cmd)
        print(bytes(stop_cmd).hex())

    def get_version(self):
        laser_version_frame = self.build_frame(0x0A)
        self.ser.write(laser_version_frame)
        version_resp = b''
        while version_resp == b'':
            version_resp = laser.ser.read_all()[-26:]
        return version_resp[7:-2].decode('ascii', 'ignore')
    
    def set_baudrate(self,data):
        '''
        data(常用，其余懒得列，具体见手册):
            bytes([0x00])    9600
            bytes([0x02])    19200
            bytes([0x05])    57600
            bytes([0x07])    115200
            bytes([0x0C])    921600
        '''
        set_baudrate_frame = self.build_frame(cmd=0x10,data=data)
        self.ser.write(set_baudrate_frame)     
        baudrate_resp = b''
        while baudrate_resp == b'':
            baudrate_resp = laser.ser.read_all()
        return (baudrate_resp[-6] << 24) | (baudrate_resp[-5] << 16) | (baudrate_resp[-4] << 8) | baudrate_resp[-3]
    
    def set_fps(self,data):
        '''
        data:
            bytes([0x00])    50
            bytes([0x01])    100
            bytes([0x02])    250

        '''
        laser_fps_frame = self.build_frame(cmd = 0x1A,data=data)
        self.ser.write(laser_fps_frame)
        fps_resp = b''
        while fps_resp == b'':
            fps_resp = laser.ser.read_all()
        if fps_resp[-3] == 0x00:
            return 50
        elif fps_resp[-3] == 0x01:
            return 100
        elif fps_resp[-3] == 0x02:
            return 250
        else:
            return -1

if __name__ =="__main__":
    try:
        laser = laserScan('/dev/ttyACM0',115200)
        print("\nStarting measurement...")
        laser.start_measure()
        while True:
            laser_get_distance = laser.get_distance()
            if laser_get_distance != -1 and laser_get_distance != -2:
                print(laser_get_distance)
            time.sleep(0.1)
    except KeyboardInterrupt:
        print("\nStopping measurement...")
        laser.stop_measure()